#ifndef URCONTROLDEPTHCAMERA_HPP
#define URCONTROLDEPTHCAMERA_HPP

//#include "d435.hpp"
#include "sr300.hpp"

#include "urcontroloffseta.hpp"

#include "ransacplane.hpp"

class URControlDepthCamera
//        : public D435
        : public SR300
        , public URControlOffsetA
{
public:
    static cv::Vec6d CalibrateDepthCamera()
    {
        double beta=CV_PI*20/180;

        cv::Matx33d rmx;
        cv::Rodrigues(cv::Vec3d(beta, 0, 0), rmx);

        cv::Matx33d rmz;

        cv::Rodrigues(cv::Vec3d(0, 0, CV_PI/2), rmz);

        cv::Vec3d rv;
        cv::Rodrigues(rmz*rmx, rv);

        cv::Vec3d offset(12+14+13*cos(beta)+24*sin(beta), -32, 34+24+12-13*sin(beta)+24*cos(beta));

        offset=offset/1000;

        return cv::Vec6d(offset[0], offset[1], offset[2], rv[0], rv[1], rv[2]);
    }


    static cv::Vec6d PointToTarget3D(cv::Vec3d targetDepthCamera, cv::Vec3d xPlusUnitDepthCamera, cv::Vec3d zPlusUnitDepthCamera,
                                     cv::Vec6d depthCameraToHead, cv::Vec6d fingerToHead, cv::Vec6d headToBase)
    {
        cv::Vec3d targetBase=HeadToBase(HeadToBase(targetDepthCamera, depthCameraToHead), headToBase);

        cv::Vec3d xPlusUnitBase=HeadToBase(HeadToBase(xPlusUnitDepthCamera, depthCameraToHead), headToBase);

        cv::Vec3d zPlusUnitBase=HeadToBase(HeadToBase(zPlusUnitDepthCamera, depthCameraToHead), headToBase);

        cv::Vec3d yPlusUnitBase=targetBase+(zPlusUnitBase-targetBase).cross(xPlusUnitBase-targetBase);

        cv::Vec3d ptBase[4]={
            targetBase,
            xPlusUnitBase,
            yPlusUnitBase,
            zPlusUnitBase
        };

        cv::Vec3d ptHead[4]={
            cv::Vec3d(0,0,0),
            cv::Vec3d(1,0,0),
            cv::Vec3d(0,1,0),
            cv::Vec3d(0,0,1)
        };

        for(int i=0;i<4;i++)
            ptHead[i]=HeadToBase(ptHead[i], fingerToHead);

        return ComputeTransform(ptHead,ptBase);
    }


//ensure unit normal vector and normal vector along z+ axis
    static cv::Vec4f GetTablePlane(const cv::Mat &xyz)
    {
        std::vector<cv::Vec3f> pt(xyz.begin<cv::Vec3f>(), xyz.end<cv::Vec3f>());

        std::cout<<pt.size()<<"->";

        for(size_t i=0;i<pt.size();)
        {
            if(pt[i][2]!=0)
            {
                i++;
            }
            else
                pt.erase(pt.begin()+i);
        }

        std::cout<<pt.size()<<'\n';

        RansacPlane rp;
        rp.Setup(pt, 0.96, 0.02);

        size_t macp=0;
        rp.Fit(macp, 16, 1000);

        std::cout<<rp.model<<'\n'<<macp<<"\n";

        //        return rp.model;


        bool* flag=new bool[pt.size()];
        double *error=new double[pt.size()];

        rp.GetInOutFlag(flag, error);

        std::vector<cv::Vec3f> ptin;
        for(size_t i=0;i<pt.size();i++)
            if(flag[i])
                ptin.push_back(pt[i]);

        cv::Vec4f plane=RansacPlane::BestPlane(ptin);

        delete []flag;
        delete []error;


        if(plane[2]<0) plane=-plane;

        std::cout<<plane<<'\n';

        return plane;
    }


    static void GetObjectMask(const cv::Mat &xyz, cv::Mat &mask, cv::Vec4f plane, float distanceThreshold)
    {
        mask.create(xyz.size(),CV_8UC1);

        for(size_t i=0;i<xyz.total();i++)
        {
            cv::Vec3f pt=xyz.at<cv::Vec3f>(i);

            if(pt[2]==0)
                mask.at<uchar>(i)=128;
            else
            {
                //                cv::Vec3d xyzBase=HeadToBase(HeadToBase(cv::Vec3d(pt[0], pt[1], pt[2]), depthCameraToHead), headToBase);

                if(RansacPlane::DistanceToPlane(pt, plane)<distanceThreshold)
                    mask.at<uchar>(i)=0;
                else
                    mask.at<uchar>(i)=255;
            }
        }
    }

    static cv::RotatedRect GetRect(const cv::Mat &mask)
    {
        //        cv::Mat bw2=mask>128;

        cv::Mat bw2=mask>0;

        cv::Mat bw_label,stat,centroid;
        cv::connectedComponentsWithStats(bw2,bw_label,stat,centroid);

        int index=1;
        for(int i=1;i<stat.rows;i++)
        {
            //         cv::Rect rc(stat.at<int>(i,cv::CC_STAT_LEFT),stat.at<int>(i,cv::CC_STAT_TOP),stat.at<int>(i,cv::CC_STAT_WIDTH),stat.at<int>(i,cv::CC_STAT_HEIGHT));
            if(stat.at<int>(i, cv::CC_STAT_AREA)>stat.at<int>(index, cv::CC_STAT_AREA))
            {
                index=i;
            }
        }

        std::vector<cv::Point> boxp;
        cv::Point pt(0,0);

        for(pt.y=0;pt.y<bw_label.rows;pt.y++)
        {
            for(pt.x=0;pt.x<bw_label.cols;pt.x++)
            {
                if(bw_label.at<int>(pt)==index)
                    boxp.push_back(pt);
            }
        }

        cv::RotatedRect rrc=cv::minAreaRect(boxp);


        std::vector<cv::Point2f> ptt(4);
        rrc.points(ptt.data());

        std::vector<cv::Point> ptt2(4);
        for(int i=0;i<4;i++)
        {
            ptt2[i].x=ptt[i].x;
            ptt2[i].y=ptt[i].y;
        }

        cv::polylines(bw2, ptt2, true, cv::Scalar(200), 2);

        cv::imshow("bw2",bw2);

        return rrc;
    }

    static cv::Vec6d PickBottle(cv::Vec3d fingerPosBase0, cv::Vec3d fingerPosBase1, cv::Vec6d fingerToHead, cv::Vec4d planeAtBase)
    {
        cv::Vec3d targetBase=(fingerPosBase0+fingerPosBase1)/2;

        cv::Vec3d zBase(planeAtBase[0],planeAtBase[1],planeAtBase[2]);
        cv::Vec3d zPlusUnitBase=zBase/cv::norm(zBase);

        cv::Vec3d yPlusUnitBase=fingerPosBase1-fingerPosBase0;
        double bottleWidth=cv::norm(yPlusUnitBase);
        yPlusUnitBase=yPlusUnitBase/bottleWidth;

        cv::Vec3d xPlusUnitBase=yPlusUnitBase.cross(zPlusUnitBase);

        cv::Vec3d ptBase[4]={
            targetBase,
            targetBase+xPlusUnitBase,
            targetBase+yPlusUnitBase,
            targetBase+zPlusUnitBase
        };

        cv::Vec3d ptHead[4]={
            cv::Vec3d(0,0,0),
            cv::Vec3d(1,0,0),
            cv::Vec3d(0,1,0),
            cv::Vec3d(0,0,1)
        };

        for(int i=0;i<4;i++)
            ptHead[i]=HeadToBase(ptHead[i], fingerToHead);

        return ComputeTransform(ptHead,ptBase);
    }

public:
    cv::Vec6d depthCameraToHead;
public:
    URControlDepthCamera()
//        : D435()
        : SR300()
        , URControlOffsetA()
    {
        depthCameraToHead=CalibrateDepthCamera();
    }



#if 0
    virtual void ProcessObject(int id, const std::vector<cv::Vec3f> &xyz)
    {
        D435::ProcessObject(id, xyz);

        if(id!=12)
            return;

        char ipUR[]="192.168.1.13";
        char ipPC[]="192.168.1.14";

        cv::Vec6d headToBase;
        GetTCPPose(ipUR, ipPC, headToBase[0], headToBase[1], headToBase[2], headToBase[3], headToBase[4], headToBase[5]);

        cv::Vec3d target(0,0,0);

        for(size_t i=0;i<4;i++)
        {
            target+=cv::Vec3d(xyz[i][0], xyz[i][1], xyz[i][2]);
        }
        target=target/4;

        cv::Vec3d dx(xyz[1][0]-xyz[0][0], xyz[1][1]-xyz[0][1], xyz[1][2]-xyz[0][2]);

        cv::Vec3d dy(xyz[3][0]-xyz[0][0], xyz[3][1]-xyz[0][1], xyz[3][2]-xyz[0][2]);

        cv::Vec3d dz=dx.cross(dy);

        dx=dx/cv::norm(dx);

        dy=dy/cv::norm(dy);

        dz=dz/cv::norm(dz);


        double bottleWidthmm=60;

        double openmm=bottleWidthmm+20;

        double fingerThickmm=37.3;

        double zOffsetmm=RG6ZOffset(bottleWidthmm/2);

        cv::Vec6d fingerToHead(0,0,0.266+0.0056,0,0,0);

        fingerToHead[2]=fingerToHead[2]+0.01;

        cv::Vec6d pose=PointToTarget3D(target, target+dy, target+dz, depthCameraToHead, fingerToHead, headToBase);

        SetTCPPose(ipUR, ipPC, pose[0], pose[1], pose[2], pose[3], pose[4], pose[5]);

        fingerToHead[2]=fingerToHead[2]-0.01;

        fingerToHead[2]=fingerToHead[2]-(zOffsetmm+bottleWidthmm/2+fingerThickmm/2)/1000;

        pose=PointToTarget3D(target, target+dy, target+dz, depthCameraToHead, fingerToHead, headToBase);

        RG6(ipUR, ipPC, openmm);

        SetTCPPose(ipUR, ipPC, pose[0], pose[1], pose[2], pose[3], pose[4], pose[5]);

        RG6(ipUR, ipPC, 0);

        SetTCPPose(ipUR, ipPC, headToBase[0], headToBase[1], headToBase[2], headToBase[3], headToBase[4], headToBase[5]);

        RG6(ipUR, ipPC, openmm);

        RG6(ipUR, ipPC, 0);

    }
#endif

#if 1
    virtual void ProcessFrame(const cv::Mat &color, const cv::Mat &ir, const cv::Mat &depth, const cv::Mat &xyz, const cv::Mat &texture, const cv::Mat &aligned_depth, const cv::Mat &aligned_xyz)
    {


        if(k=='t')
        {
            cv::Vec4f planeAtCamera=GetTablePlane(aligned_xyz);

            char ipUR[]="192.168.1.13";
            char ipPC[]="192.168.1.14";

            cv::Vec6d headToBase;
            GetTCPPose(ipUR, ipPC, headToBase[0], headToBase[1], headToBase[2], headToBase[3], headToBase[4], headToBase[5]);

            cv::Vec6d depthCameraToBase=ComposeTransform(depthCameraToHead, headToBase);

            cv::Vec4d planeAtBase=HeadToBase(cv::Vec4d(planeAtCamera[0], planeAtCamera[1], planeAtCamera[2], planeAtCamera[3]), depthCameraToBase);

            std::cout<<"planeAtBase="<<planeAtBase<<"\n"<<std::flush;
        }

        if(k=='c')
        {
            char ipUR[]="192.168.1.13";
            char ipPC[]="192.168.1.14";

            cv::Vec4d planeAtBase(0.0181312, -0.0304963, 0.99937, -0.890918);

            cv::Vec6d fingerToHead(0,0,0.266+0.0056,0,0,0);

            PickBottle(planeAtBase, fingerToHead, aligned_xyz, 0.02, ipUR, ipPC, 0.02, 0.2);
        }




        // Display in a GUI
        cv::namedWindow("color Image", cv::WINDOW_AUTOSIZE );
        cv::imshow("color Image", color);
        cv::namedWindow("aligned depth Image ColorMap", cv::WINDOW_AUTOSIZE );
        cv::imshow("aligned depth Image ColorMap", ColorMap(aligned_depth));
        k=cv::waitKey(1);
    }
#endif

    double GetFingerPosition(cv::Vec3f &fingerPos0, cv::Vec3f &fingerPos1, cv::RotatedRect objectOnFrame, cv::Vec4f plane,
                             double widthToVisibleHeightRatio=2, double visibleHeightToHeightRatio=0.5)
    {
        cv::Point2f pt[4];

        objectOnFrame.points(pt);

        cv::Point2f fingerPosOnFrame[2];

        if(cv::norm(pt[0]-pt[1])>cv::norm(pt[0]-pt[3]))
        {
            fingerPosOnFrame[0]=(pt[0]+pt[1])/2;
            fingerPosOnFrame[1]=(pt[2]+pt[3])/2;
        }
        else
        {
            fingerPosOnFrame[0]=(pt[0]+pt[3])/2;
            fingerPosOnFrame[1]=(pt[2]+pt[1])/2;
        }

        std::cout<< "fingerPosOnFrame[0]="<<fingerPosOnFrame[0]<<"\n";
        std::cout<< "fingerPosOnFrame[1]="<<fingerPosOnFrame[1]<<"\n";

        cv::Vec3f fingerRay0=Deproject(cv::Vec2f(fingerPosOnFrame[0].x, fingerPosOnFrame[0].y));
        cv::Vec3f fingerRay1=Deproject(cv::Vec2f(fingerPosOnFrame[1].x, fingerPosOnFrame[1].y));

        std::cout<< "fingerRay0="<<fingerRay0<<"\n";
        std::cout<< "fingerRay1="<<fingerRay1<<"\n";


        cv::Vec3f normal(plane[0],plane[1],plane[2]);//should be unit vector and along to z+ axis

        //        double a0=normal.ddot(fingerRay0);
        //        double a1=normal.ddot(fingerRay1);

        cv::Vec3f fingerRay0n=fingerRay0/(normal.ddot(fingerRay0));

        cv::Vec3f fingerRay1n=fingerRay1/(normal.ddot(fingerRay1));

        std::cout<< "fingerRay0n="<<fingerRay0n<<"\n";
        std::cout<< "fingerRay1n="<<fingerRay1n<<"\n";

        cv::Vec3f dfVector=fingerRay0n-fingerRay1n;

        std::cout<< "dfVector="<<dfVector<<"\n";

        double dfLength=cv::norm(dfVector);

        double newd=plane[3]/(dfLength/widthToVisibleHeightRatio+1);

        double visibleHeight=fabs(plane[3]-newd);


        fingerPos0=-newd*fingerRay0n+(1-0.5/visibleHeightToHeightRatio)*visibleHeight*normal;
        fingerPos1=-newd*fingerRay1n+(1-0.5/visibleHeightToHeightRatio)*visibleHeight*normal;

        double width=visibleHeight*widthToVisibleHeightRatio;

        std::cout<< "fingerPos0="<<fingerPos0<<"\n";
        std::cout<< "fingerPos1="<<fingerPos1<<"\n";

        std::cout<< "dfn="<<cv::norm(fingerPos0-fingerPos1)<<"\n";

        std::cout<< "dfn2="<<width<<"\n";


        return width;
    }

    double GetFingerPosition(cv::Vec3f &fingerPos0, cv::Vec3f &fingerPos1, const cv::Mat &xyz, cv::Vec4f tablePlane, float distanceThreshold)
    {
        cv::Mat mask;
        GetObjectMask(xyz, mask, tablePlane, distanceThreshold);
        cv::imshow("mask", mask);
        return GetFingerPosition(fingerPos0, fingerPos1, GetRect(mask), tablePlane, 1, 1);
    }

    void PickBottle(cv::Vec4d planeAtBase, cv::Vec6d fingerToHead, const cv::Mat &xyz, double distanceThreshold, const char *ipUR, const char *ipPC, double a, double v)
    {
        cv::Vec6d headToBase;
        GetTCPPose(ipUR, ipPC, headToBase[0], headToBase[1], headToBase[2], headToBase[3], headToBase[4], headToBase[5]);

        cv::Vec6d depthCameraToBase=ComposeTransform(depthCameraToHead, headToBase);

        cv::Vec6d baseToDepthCamera=ReverseTransform(depthCameraToBase);

        cv::Vec4d planeAtCamera=HeadToBase(planeAtBase, baseToDepthCamera);

        cv::Vec3f fpos[2];
        double bottleWidth=GetFingerPosition(fpos[0], fpos[1], xyz, planeAtCamera, distanceThreshold);

        cv::Vec3d fingerPosBase[2];

        fingerPosBase[0]=HeadToBase(cv::Vec3d(fpos[0][0], fpos[0][1], fpos[0][2]), depthCameraToBase);
        fingerPosBase[1]=HeadToBase(cv::Vec3d(fpos[1][0], fpos[1][1], fpos[1][2]), depthCameraToBase);

        double bottleWidthmm=bottleWidth*1000;

        double openmm=bottleWidthmm+20;

        double fingerThickmm=37.3;

        double zOffsetmm=RG6ZOffset(bottleWidthmm/2);

        fingerToHead[2]=fingerToHead[2]+0.01+bottleWidth/2;

        cv::Vec6d pose=PickBottle(fingerPosBase[0], fingerPosBase[1], fingerToHead, planeAtBase);

        SetTCPPose(ipUR, ipPC, pose[0], pose[1], pose[2], pose[3], pose[4], pose[5], a, v);

        fingerToHead[2]=fingerToHead[2]-0.01-bottleWidth/2;

        fingerToHead[2]=fingerToHead[2]-(zOffsetmm+fingerThickmm/2)/1000;

        pose=PickBottle(fingerPosBase[0], fingerPosBase[1], fingerToHead, planeAtBase);

        RG6(ipUR, ipPC, openmm);


        SetTCPPose(ipUR, ipPC, pose[0], pose[1], pose[2], pose[3], pose[4], pose[5], a, v);


        RG6(ipUR, ipPC, 0);


        SetTCPPose(ipUR, ipPC, headToBase[0], headToBase[1], headToBase[2], headToBase[3], headToBase[4], headToBase[5], a, v);


        RG6(ipUR, ipPC, openmm);

        RG6(ipUR, ipPC, 0);
    }



};


#endif // URCONTROLDEPTHCAMERA_HPP

